#include "lib/contour.h"

namespace behavior_controller {
ContourAreas contour_areas;

bool Contour::IsPointInShape(const PointF &point) {
    if (!cv_creat) {
        std::cout << "CreatCvShape !" << std::endl;
        CreatCvShape();
        cv_creat = true;
    }
    cv::Point2f test_point;
    test_point.x = point.x();
    test_point.y = point.y();
    if (cv_contour.empty()) {
        std::cout << "cv_contour empty!" << std::endl;
        return false;
    }
    return (cv::pointPolygonTest(cv_contour, test_point, false) > 0);
}

void Contour::resize(uint n) {
    contour_points_.clear();
    contour_points_.resize(n);
    cv_contour.clear();
    cv_contour.resize(n);

}

void Contour::CreatCvShape() {
    cv::Point2f cv_point;
    if (contour_points_.size() != cv_contour.size()) {
        cv_contour.clear();
        cv_contour.resize(contour_points_.size());
    }
    for (int i = 0; i < contour_points_.size(); i++) {
        cv_point.x = contour_points_[i].x();
        cv_point.y = contour_points_[i].y();
        cv_contour[i] = cv_point;
        std::cout << "cv_point.x= " << cv_contour[i].x << "  cv_point.y=" << cv_contour[i].y << std::endl;
    }
}

const std::vector<PointF> &Contour::contour_points() const {
    return contour_points_;
}

std::vector<PointF> &Contour::contour_points() {
    return contour_points_;
}

void Contour::ModifyContour(int id, const PointF point) {
    contour_points_[id] = point;
}

void ContourAreas::Load(const YAML::Node &contour_yaml_) {
    robot_shape_ = contour_yaml_["robot_shape"].as<Contour>();
    front_area_ = contour_yaml_["front_area"].as<Contour>();
    back_area_ = contour_yaml_["back_area"].as<Contour>();
}

Contour &ContourAreas::robot_shape() {
    return robot_shape_;
}

Contour &ContourAreas::front_area() {
    return front_area_;
}

Contour &ContourAreas::back_area() {
    return back_area_;
}

} // namespace behavior_controller
